#include "Matrix4.h"
#include <GL/gl.h>
#include <cmath>

Matrix4::Matrix4() {
  for(int index = 0; index < 16; index++) values[index] = 0.0;
  values[0] = 1.0;
  values[5] = 1.0;
  values[10] = 1.0;
  values[15] = 1.0;
}

Matrix4::Matrix4(const Matrix4 & other) {
  for(int index = 0; index < 16; index++)
    values[index] = other.values[index];
}

Matrix4 & Matrix4::operator=(const Matrix4 & other) {
  for(int index = 0; index < 16; index++)
    values[index] = other.values[index];

  return *this;
}

Matrix4 Matrix4::operator+(const Matrix4 & other) const {
  Matrix4 result;

  for(int index = 0; index < 16; index++) {
    result.values[index] = values[index] + other.values[index];
  }

  return result;
}

Matrix4 Matrix4::operator-(const Matrix4 & other) const {
  Matrix4 result;

  for(int index = 0; index < 16; index++) {
    result.values[index] = values[index] - other.values[index];
  }

  return result;
}

Matrix4 Matrix4::operator*(const Matrix4 & other) const {
  Matrix4 result;

  for(int i = 0; i < 4; i++)
  for(int j = 0; j < 4; j++) {
    result(i, j) = 0.0;
    for(int k = 0; k < 4; k++) {
      result(i, j) += (*this)(i, k) * other(k, j);
    }
  }

  return result;
}

Vector3 Matrix4::operator*(const Vector3 & vector) const {
  float temp[4] = {vector.x, vector.y, vector.z, 0.0};
  float result[4] = {0.0, 0.0, 0.0, 0.0};

  for(int i = 0; i < 4; i++)
  for(int j = 0; j < 4; j++) {
    result[i] += (*this)(i, j) * temp[j];
  }

  return Vector3(result[0], result[1], result[2]);
}

Point3 Matrix4::operator*(const Point3 & vector) const {
  float temp[4] = {vector.x, vector.y, vector.z, 1.0};
  float result[4] = {0.0, 0.0, 0.0, 0.0};

  for(int i = 0; i < 4; i++)
  for(int j = 0; j < 4; j++) {
    result[i] += (*this)(i, j) * temp[j];
  }

  return Point3(result[0], result[1], result[2]);
}

Matrix4 Matrix4::transpose() const {
  Matrix4 result;

  for(int i = 0; i < 4; i++)
  for(int j = 0; j < 4; j++) {
    result(i, j) = (*this)(j, i);
  }

  return result;
}

float& Matrix4::operator()(unsigned int row, unsigned int column) {
  return values[column * 4 + row];
}

const float& Matrix4::operator()(unsigned int row, unsigned int column) const {
  return values[column * 4 + row];
}

void Matrix4::applyGL() const {
  glMultMatrixf(values);
}

Matrix4 rotationMatrix(float angle, Vector3 axis) {
  axis = axis.normalize();

  Matrix4 xRotation, yRotation, zRotation;
  float d = sqrt(pow(axis.y, 2.0) + pow(axis.z, 2.0));

  if(fabs(d > 0.0001)) {
    xRotation(1,1) = axis.z/d;
    xRotation(2,2) = axis.z/d;
    xRotation(1,2) = -axis.y/d;
    xRotation(2,1) = axis.y/d;
  }

  yRotation(0,0) = d;
  yRotation(2,2) = d;
  yRotation(0,2) = -axis.x;
  yRotation(2,0) = axis.x;

  zRotation(0,0) = cos(angle);
  zRotation(1,1) = cos(angle);
  zRotation(0,1) = -sin(angle);
  zRotation(1,0) = sin(angle);

  return xRotation.transpose() * yRotation.transpose() * zRotation * yRotation * xRotation;
}

Matrix4 translationMatrix(Vector3 displacement) {
  Matrix4 result;
  result(0, 3) = displacement.x;
  result(1, 3) = displacement.y;
  result(2, 3) = displacement.z;

  return result;
}

Matrix4 scaleMatrix(float x, float y, float z) {
  Matrix4 result;
  result(0, 0) = x;
  result(1, 1) = y;
  result(2, 2) = z;

  return result;
}

Matrix4 scaleMatrix(float scale) {
  Matrix4 result;
  result(0, 0) = scale;
  result(1, 1) = scale;
  result(2, 2) = scale;

  return result;
}

Matrix4 shearMatrix(float angle_grau) {
  Matrix4 result;
  result(2,1) = -1/tan(rad(angle_grau));

  return result;
}

double rad(double angulo)
{
    return(angulo*(M_PI/180));
}
